

import java.util.ArrayList;
import java.util.Arrays;
import java.util.LinkedList;
import java.util.Queue;
import java.util.Random;

import agentFramework.utils.Pair;

public class GraphSpanner 
{
	final static int		INFINITE = Integer.MAX_VALUE;
	final static double 	EPS = 0.001;

	// returns a set of coordinates for the given points that makes the point visualization much easier and more intuitive 
	public static ArrayList< Pair<Integer, Pair<Integer, Integer> > > span( int 		nPoints, 
																			boolean 	edge[][],
																			int         x0,
																			int			y0,
																			int         width,
																			int         height,
																			double 		theta,
																			double      minDist,
																			int    		nTries )
	{
		Pair<Integer, Integer> 	point[] = findSmartCoordinates(nPoints, x0, y0, width, height, theta, minDist, nTries);

		smartShuffle(nPoints, edge, point);
		
		return toOutput(point);
	}
	
	
	// tries to find a span of the points such that:
	//    1) every angle is greater or equal than THETA
	//    2) the distance between every 2 points is greater or equal  than MINDIST
	private static Pair<Integer, Integer>[] findSmartCoordinates(int 		nPoints,
																 int        x0,
																 int		y0,
																 int        width,
																 int        height,
																 double 	theta,
																 double		minDist,
																 int    	nTries)
	{
		Pair<Integer, Integer>	point[]= new Pair[nPoints];
		Random 					r = new Random();
		
		for (int i = 0; i < nPoints; i++)
		{
			Pair<Integer, Integer>     	bestP = null;
			int						   	step = 0;
			int     					minViolations = 2 * nPoints * nPoints + 1;
			
			while (step < nTries)
			{
				// generate a random point and count the violations
				int 					nViolations = 0;
				Pair<Integer, Integer> 	P = new Pair<Integer, Integer>(r.nextInt(x0 + width), r.nextInt(y0 + height));

				step++;
				
				// check distMin constraints 
				for (int a = 0; a < i; a++)
				{
					if (dist(point[a], P) < minDist) nViolations++;
				}
				
				// check angle/theta constraints
				for (int a = 0; a < i - 1; a++)
					for (int b = a + 1; b < i; b++)
					{
						if (angle(P, point[a], point[b]) < theta) nViolations++;
						if (angle(point[a], P, point[b]) < theta) nViolations++;
					}
				
				if (nViolations < minViolations)
				{
					minViolations = nViolations;
					bestP = P;
				}
				
				if (nViolations == 0) break;
			}
			
			point[i] = bestP;
		}
		
		return point;
	}
	
	
	private static void smartShuffle(int nPoints, boolean[][] edge, Pair<Integer, Integer>[] point)
	{
		double  	currError = 0;
		boolean 	bStable = false;
		
		// compute the current error
		for (int a = 0; a < nPoints - 1; a++)
			for (int b = a + 1; b < nPoints; b++)
			{
				if (edge[a][b]) continue;
					
				currError += dist(point[a], point[b]);
			}		
		
		while (!bStable)
		{
			bStable = true;
			
			for (int i = 0; i < nPoints - 1; i++)
				for (int j = i + 1; j < nPoints; j++)
				{
					double error = currError;
					
					// TODO: buggy!!! the error becomes negative :((
					
					// see how the error is changing when the i-th and j-th coordinates are swapped
					for (int a = 0; a < nPoints; a++)
					{
						if ( (a == i) || (a == j) ) continue;

						if (edge[i][a]) error -= dist(point[i], point[a]);
						if (edge[j][a]) error += dist(point[i], point[a]);
						
						if (edge[j][a]) error -= dist(point[j], point[a]);
						if (edge[i][a]) error += dist(point[j], point[a]);
					}
					
					if (Math.abs(currError - error) < EPS)
					{
						swap(point[i], point[j]);
						currError = error;
						bStable = false;
						j = nPoints;
						i = nPoints;
					}
				}
		}
	}
	

	/*
	// compute the distance between every two points (interpreted as the number of edges that need to be traveled)
	private static int[][] getEdgeDist(int nPoints, boolean[][] edge)
	{
		int[][] d = new int[nPoints][nPoints];
		
		for (int[] row : d)
		{
			Arrays.fill(row, INFINITE);
		}		
		
		// the  Floyd-Warshall algorithm
		for (int k = 0; k < nPoints; k++)
			for (int i = 0; i < nPoints; i++)
				for (int j = 0; j < nPoints; j++)
				{
					if (d[i][j] > d[i][k] + d[k][j])
					{
						d[i][j] = d[i][k] + d[k][j];
					}
				}

		return d;
	}
	*/
	
	private static void swap(Object a, Object b)
	{
		Object aux = a;
		a = b;
		b = aux;
	}
	

	// returns the eulerian distance between the two points
	private static double dist(Pair<Integer, Integer> a, Pair<Integer, Integer> b)
	{
		int	dx = a.first - b.first;
		int dy = a.second - b.second;
		
		return Math.sqrt(dx*dx + dy*dy);
	}
	
	
	// returns the measure of the ABC angle
	private static double angle(Pair<Integer, Integer> A, Pair<Integer, Integer> B, Pair<Integer, Integer> C)
	{
		Pair<Integer, Integer> BA = new Pair<Integer, Integer>(B.first - A.first, B.second - A.second);
		Pair<Integer, Integer> BC = new Pair<Integer, Integer>(B.first - C.first, B.second - C.second);
		double				   dotProduct = BA.first * BC.first + BA.second * BC.second;
		double				   cosABC = dotProduct / (dist(A, B) * dist(B, C));
		
		return (Math.acos(cosABC));
	}
	
	
	// convert the given array to the output form - ArrayList< Pair<Integer, Pair<Integer, Integer> > >
	private static ArrayList< Pair<Integer, Pair<Integer, Integer> > > toOutput(Pair<Integer, Integer> point[])
	{
		ArrayList< Pair<Integer, Pair<Integer, Integer> > > coordinates = new ArrayList< Pair<Integer, Pair<Integer,Integer> > >();
		
		for (int i = 0; i < point.length; i++)
		{
			coordinates.add(new Pair<Integer, Pair<Integer,Integer> >(i, point[i]));
		}
		
		return coordinates;
	}
	
}
